"""
投放引导，处理飞行器最末段的寻径和到达时间预测

date: 2025/8/5
author: SiHeng Tang
file: release_path.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
from helpers.geo_math_trans import GeoVector
from mj_errors import MJRuntimeError


class _PositionQueue:
    """
    位置队列，针对 PA 的特殊简易队列
    位置队列，针对 PA 的特殊简易队列
    """

    def __init__(self, lim: int):
        self._data: list[GeoVector] = []
        self._lim = lim

    def put(self, item: GeoVector):
        # 处理超长，直接放弃没有使用的旧值
        if len(self) >= self._lim:
            self._data = self._data[1:]
        self._data.append(item)

    def get(self) -> list[GeoVector]:
        # get 函数直接返回当前的所有值，这是对 PA 的定制
        return self._data[0:len(self._data)]

    def __len__(self):
        return len(self._data)


class _BasePathAdviser:
    def set_target(self, pos: GeoVector):
        pass

    def update_path(self, predicted_point: GeoVector):
        pass

    def get_next_advice(self, now_hdg: float):
        pass


class _LegacyPathAdvice(_BasePathAdviser):
    """
    投放路径引导，简而言之这是一个控制器，从落点预测推算航向。
    使用的每一个帧都必须是等长的，应该使用 history 之类的模块缓冲从飞行器遥测获得的初始值。
    使用等长的帧间隔的主要原因是稳定 PID 参数，目前此类不支持从时间推断，默认从 history 缓冲。
    目前建议 2Hz
    """
    default_params = {
        "target_lat": 0,
        "target_lon": 0,
        "min_advice_lim": 5,
        "min_history_len": 6,
        "ctl_kp": 0.3,
        "ctl_ki": 0.0,
        "ctl_kd": 0.0
    }

    def __init__(self, params: dict):
        """
        初始化路径观测器
        :param tar_lat: 目标纬度
        :param tar_lon: 目标经度
        :param ctl_pid_set: 控制器 PID 系数，有序列表，[K_p, K_i, K_d]
        :param approach_lim: 接近距离终止引导阈值，与目标距离小于此参数不再更新航向
        :param pos_length: 预测使用的历史落点数，默认为 6
        """
        self.default_params.update(params)

        if (self.default_params["min_history_len"] % 2 != 0) or (self.default_params["min_history_len"] < 2):
            raise ValueError("Can not accept to low position history length.")

        self.target_pos: GeoVector = GeoVector(lat=self.default_params["target_lat"],
                                               lon=self.default_params["target_lat"])
        self.history = _PositionQueue(self.default_params["min_history_len"])
        self.pos_length = self.default_params["min_history_len"]
        self.approach_lim = self.default_params["min_advice_lim"]

        # 比例积分微分控制器参数
        self.ctl_int: float = 0.0
        self.ctl_diff: float = 0.0
        self.ctl_last_diff: float = 0.0
        self.ctl_is_init = False
        self.CTL_KP = self.default_params["ctl_kp"]
        self.CTL_KI = self.default_params["ctl_ki"]
        self.CTL_KD = self.default_params["ctl_kd"]

    def set_target(self, pos: GeoVector):
        self.target_pos = pos

    def update_path(self, predicted_point: GeoVector):
        """
        刷新投放观测器
        :param predicted_point: 通过落点预测方法得到的预计落点
        :return: 航向角，距离，时间
        """
        self.history.put(predicted_point)

    # 航向控制器先计算落点移动航向，计算目标计算目标绝对航向与当前航向的差值作为 PID 控制器差输入
    # PID 控制器输出一个偏航增量，使用这种方法避免过去的矢量计算在角度输出上非线性的问题
    def get_next_advice(self, hdg: float) -> tuple[float, float]:
        """
        获取下一个建议的航向，如果预测失败，那么剩余时间和距离为 0
        :param hdg: 航向角
        :return: 航向角，距离，时间
        """
        # 是否达到开始预测所需历史长度
        if len(self.history.get()) != self.pos_length:
            return hdg, 0

        # 如果落点已经靠近目标位置，引导器终止更新，防止出现数学问题
        length = (self.history.get()[-1].diff(self.target_pos).length())
        if length <= self.approach_lim:
            return hdg, 0

        # 估算平均增量
        diffs = []
        avg_diff = GeoVector(dx=0, dy=0)
        drop_points = self.history.get()
        for i in range(int(self.pos_length / 2)):
            diffs.append(drop_points[i].diff(drop_points[self.pos_length - 1 - i]))
        for gv in diffs:
            avg_diff = avg_diff + gv

        # 没有速度
        if avg_diff.length() < 1e-2:
            return hdg, 0

        # 计算出一个从速度单位矢量指向目标方向单位矢量的矢量
        direct_delta = drop_points[-1].diff(self.target_pos)
        diff = GeoVector.get_hdg_from_gv(direct_delta) - GeoVector.get_hdg_from_gv(avg_diff)

        # 更新比例积分微分控制器
        if not self.ctl_is_init:
            self.ctl_last_diff = diff
            self.ctl_int = diff
            self.ctl_is_init = True
            return hdg, 0
        self.ctl_int = (self.ctl_int + diff) % 360.0
        self.ctl_diff = diff - self.ctl_last_diff
        self.ctl_last_diff = diff

        # 控制器，通过落点变化趋势和目标方向的矢量差，推定目前航向角的偏移量
        new_v = hdg + diff * self.CTL_KP + self.ctl_int * self.CTL_KI + self.ctl_diff * self.CTL_KD
        return new_v, float(length)


class _VerySimplePath(_BasePathAdviser):
    """
    投放路径引导，简而言之这是一个控制器，从落点预测推算航向。
    使用的每一个帧都必须是等长的，应该使用 history 之类的模块缓冲从飞行器遥测获得的初始值。
    使用等长的帧间隔的主要原因是稳定 PID 参数，目前此类不支持从时间推断，默认从 history 缓冲。
    目前建议 2Hz
    """
    default_params = {
        "target_lat": 0,
        "target_lon": 0,
        "min_history_len": 6,
        "min_advice_lim": 5
    }

    def __init__(self, params: dict):
        self.default_params.update(params)

        if (self.default_params["min_history_len"] % 2 != 0) or (self.default_params["min_history_len"] < 2):
            raise ValueError("Can not accept to low position history length.")

        self.target_pos: GeoVector = GeoVector(lat=self.default_params["target_lat"],
                                               lon=self.default_params["target_lon"])
        self.history = _PositionQueue(self.default_params["min_history_len"])
        self.pos_length = self.default_params["min_history_len"]
        self.approach_lim = self.default_params["min_advice_lim"]

    def set_target(self, pos: GeoVector):
        self.target_pos = pos

    def update_path(self, predicted_point: GeoVector):
        """
        刷新投放观测器
        :param predicted_point: 通过落点预测方法得到的预计落点
        :return: 航向角，距离，时间
        """
        self.history.put(predicted_point)

    # 你可能觉得上一个 PID 控制器太花哨了，我也这么觉得，不如让我们直接对准目标
    def get_next_advice(self, hdg: float) -> tuple[float, float]:
        """
        获取下一个建议的航向，如果预测失败，那么剩余距离为 0
        :param hdg: 航向角
        :return: 航向角，距离
        """
        # 是否达到开始预测所需历史长度
        if len(self.history.get()) != self.pos_length:
            return hdg, 0

        # 如果落点已经靠近目标位置，引导器终止更新，防止出现数学问题
        length = (self.history.get()[-1].diff(self.target_pos).length())
        if length <= self.approach_lim:
            return hdg, 0

        # 估算平均增量
        diffs = []
        avg_diff = GeoVector(dx=0, dy=0)
        drop_points = self.history.get()
        for i in range(int(self.pos_length / 2)):
            diffs.append(drop_points[i].diff(drop_points[self.pos_length - 1 - i]))
        for gv in diffs:
            avg_diff = avg_diff + gv

        # 没有速度
        if avg_diff.length() < 1e-2:
            return hdg, 0

        # 控制器，通过落点变化趋势和目标方向的矢量差，推定目前航向角的偏移量
        return GeoVector.get_hdg_from_gv(drop_points[-1].diff(self.target_pos)), float(length)


ALL_ADVISER = {
    "legacy": _LegacyPathAdvice,
    "direct": _VerySimplePath
}


def get_path_adviser(name: str, adviser_args: dict):
    if name not in ALL_ADVISER:
        raise MJRuntimeError(MJRuntimeError.PROCESSOR_ERROR, f"{name} is an invalid PA name")
    return ALL_ADVISER.get(name)(adviser_args)
